from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='can_motor_driver',
            executable='winch_can_motor_driver_node',
            name='winch_can_motor_driver_node',
            output='screen',
            parameters=[
                {'serial_port_name': '/dev/ttyMotor'},
                {'control_frequency': 50.0},
                {'torque_filter_time': 1.0}
            ]
        ),
        Node(
            package='can_motor_driver',
            executable='winch_wireless_server_v2',
            name='winch_wireless_server_v2',
            output='screen',
            parameters=[
                {'serial_port_name': '/dev/ttyWireless'},
                {'control_frequency': 5.0},
                # {'torque_filter_time': 1.0}
            ]
        ),
        Node(
            package='winch_ctrl',
            executable='winch_ctrl',
            name='winch_ctrl',
            output='screen',
            parameters=[
                {'controller_freq': 200.0},
                {'MANUAL_SPEED': 1.0},
                {'accleration': 0.2},
                {'set_length_speed': 1.0},
                {'max_velocity': 1.0},
                {'min_velocity': 0.2},
                {'hold_speed': 0.135},
                {'critical_d_torque': -0.5},
                {'precise_zero_step_min': 0.01},
                {'precise_zero_step_max': 0.030},
                {'swimming_torque': 0.7},
                {'BACK_TORQUE': -0.3},
                {'fly_height': 20.0}, 
                {'tag_optimal_distance':1.0}, 
                {'reservedHeight': 1.0} 
                
            ]
        ),
        Node(
            package='state_monitor',
            executable='gcs_client_node_winch',
            name='gcs_client_node_winch',
            output='screen'
        ),
        Node(
            package='state_monitor',
            executable='restart_node_winch',
            name='restart_node_winch',
            output='screen'
        ),
        Node(
            package='can_motor_driver',
            executable='gpio_driver',
            name='gpio_driver',
            output='screen',
           
        ),

        Node(
            package='dji_psdk_ros2',
            executable='dji_psdk_ros2',
            name='dji_psdk_ros2',
            output='screen'
        )

        # Node(
        #     package='can_motor_driver',
        #     executable='gpio_driver',
        #     name='gpio_driver',
        #     output='screen',
        #     parameters=[
        #         {'gpio_port_name': '/dev/ttyGPIO'},
        #     ]
        # ),
        # Node(
        #     package='socket_bridge',
        #     executable='gcs_cmd_publisher',
        #     name='gcs_cmd_publisher',
        #     output='screen'
        # ),
        # Node(
        #     package='socket_bridge',
        #     executable='drone_whole_state_receiver',
        #     name='drone_whole_state_receiver',
        #     output='screen'
        # )
        # ),
        # Node(
        #     package='nlink_parser',
        #     executable='tofsensem',
        #     name='tofsensem0',
        #     output='screen',
        #     parameters=[
        #         {'port_name': '/dev/ttyS7'},
        #         {'baud_rate': 921600}
        #     ]
        # ),
        # Node(
        #     package='tof_detector',
        #     executable='tof_detector',
        #     name='tof_detector',
        #     output='screen',
        #     parameters=[
        #         {'emergency_stop_enable': False},
        #         {'critical_control_error': 0.15},
        #         {'critical_tof_distance': 0.4}
        #     ]
        # )
    ])

if __name__ == '__main__':
    generate_launch_description()
